#include "motec.h"

static int init_step = 0;
UNS8 motec_status[2] = {0,0};


void motec_init(void)
{
	ObjDict_Data.heartbeatError = Master_heartbeatError;
	ObjDict_Data.initialisation = Master_initialisation;
	ObjDict_Data.preOperational = Master_preOperational;
	ObjDict_Data.operational = Master_operational;
	ObjDict_Data.stopped = Master_stopped;
	ObjDict_Data.post_sync = Master_post_sync;
	ObjDict_Data.post_TPDO = Master_post_TPDO;
	ObjDict_Data.post_emcy = Master_post_emcy;
	ObjDict_Data.post_SlaveBootup = Master_post_SlaveBootup;
	ObjDict_Data.post_SlaveStateChange = Master_post_SlaveStateChange;
	
	//canfestival
	canInit(&ObjDict_Data, CAN_BAUDRATE);         		// Initialize the CANopen bus
  initTimer();                                 	// Start timer for the CANopen stack
	
	delay_ms(500);
	setState(&ObjDict_Data, Pre_operational);
	setState(&ObjDict_Data, Operational);
}

void ConfigureSlaveLoop()
{
	if(ObjDict_Data.NMTable[1] == Pre_operational)
	{
		check_and_start_node(&ObjDict_Data, 1);
	}
	if(ObjDict_Data.NMTable[2] == Pre_operational)
	{
		check_and_start_node(&ObjDict_Data, 2);
	}
}

UNS8 ReadSDO(UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, void* data, UNS32* size)
{
	UNS32 abortCode = 0;
	UNS8 res = SDO_UPLOAD_IN_PROGRESS;
	// Read SDO
	UNS8 err = readNetworkDict (&ObjDict_Data, nodeId, index, subIndex, dataType, 0);
	if(err)
		return 0xFF;
	for(;;)
	{
		res = getReadResultNetworkDict (&ObjDict_Data, nodeId, data, size, &abortCode);
		if (res != SDO_UPLOAD_IN_PROGRESS)
			 break;
		continue;
	}
	closeSDOtransfer(&ObjDict_Data, nodeId, SDO_CLIENT);
	if (res == SDO_FINISHED)
		return 0;
	return 0xFF;   
}
	 
/*
UNS32 OnMasterMap1Update(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
{
	;//eprintf("OnMasterMap1Update:%d\n", MasterMap1);
	return 0;
}*/

/**/
static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
{
	UNS32 abortCode;

	if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) 
	{
		//eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
	}
	/* Finalise last SDO transfer with this node */
	closeSDOtransfer(&ObjDict_Data, nodeId, SDO_CLIENT);

	ConfigureSlaveNode(d, nodeId);
}

/********************************************************
 * ConfigureSlaveNode is responsible to
 *  - setup slave TPDO 1 transmit time
 *  - setup slave TPDO 2 transmit time
 *  - setup slave Heartbeat Producer time
 *  - switch to operational mode
 *  - send NMT to slave
 ********************************************************
 * This an example of :
 * Network Dictionary Access (SDO) with Callback 
 * Slave node state change request (NMT) 
 ********************************************************
 * This is called first by TestMaster_preOperational
 * then it called again each time a SDO exchange is
 * finished.
 ********************************************************/
static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
{
	UNS8 res;
	switch(++init_step){
		case 1: 
		{	/*disable Slave's TPDO 1 */
			UNS32 TPDO_COBId = 0x00000080 + nodeId + 0x100*nodeId;
			//UNS32 TPDO_COBId = 0x0;
			res = writeNetworkDictCallBack(	d, /*CO_Data* d*/
																			/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
																			nodeId, /*UNS8 nodeId*/
																			0x1800, /*UNS16 index*/
																			0x01, /*UNS8 subindex*/
																			4, /*UNS8 count*/
																			0, /*UNS8 dataType*/
																			&TPDO_COBId,/*void *data*/
																			CheckSDOAndContinue, /*SDOCallback_t Callback*/
																			0); /* use block mode */
    }
		break;

		case 2: 
		{	/*setup Slave's TPDO 1 to be transmitted on SYNC*/
			UNS8 Transmission_Type = 0x0;
			res = writeNetworkDictCallBack(	d, /*CO_Data* d*/
																			/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
																			nodeId, /*UNS8 nodeId*/
																			0x1800, /*UNS16 index*/
																			0x02, /*UNS8 subindex*/
																			1, /*UNS8 count*/
																			0, /*UNS8 dataType*/
																			&Transmission_Type,/*void *data*/
																			CheckSDOAndContinue, /*SDOCallback_t Callback*/
																			0); /* use block mode */
		}			
		break;

		case 3: 
		{	/*re-enable Slave's TPDO 1 */
			UNS16 Event_Timer = 30;
			res = writeNetworkDictCallBack(	d, /*CO_Data* d*/
																			/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
																			nodeId, /*UNS8 nodeId*/
																			0x1800, /*UNS16 index*/
																			0x05, /*UNS8 subindex*/
																			2, /*UNS8 count*/
																			0, /*UNS8 dataType*/
																			&Event_Timer,/*void *data*/
																			CheckSDOAndContinue, /*SDOCallback_t Callback*/
																			0); /* use block mode */
		}			
		break;
		
		case 4: 
		{	/*re-enable Slave's TPDO 1 */
			UNS32 Entries_Number = 0x1;
			res = writeNetworkDictCallBack(	d, /*CO_Data* d*/
																			/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
																			nodeId, /*UNS8 nodeId*/
																			0x1A00, /*UNS16 index*/
																			0x0, /*UNS8 subindex*/
																			1, /*UNS8 count*/
																			0, /*UNS8 dataType*/
																			&Entries_Number,/*void *data*/
																			CheckSDOAndContinue, /*SDOCallback_t Callback*/
																			0); /* use block mode */
		}			
		break;
		
		case 5: 
		{	/*re-enable Slave's TPDO 1 */
			UNS32 Transmit_Mapping = 0x606C0010;
			res = writeNetworkDictCallBack(	d, /*CO_Data* d*/
																			/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
																			nodeId, /*UNS8 nodeId*/
																			0x1A00, /*UNS16 index*/
																			0x01, /*UNS8 subindex*/
																			4, /*UNS8 count*/
																			0, /*UNS8 dataType*/
																			&Transmit_Mapping,/*void *data*/
																			CheckSDOAndContinue, /*SDOCallback_t Callback*/
																			0); /* use block mode */
		}			
		break;
		
		case 6: 
		{	/*re-enable Slave's RPDO 1 */
			UNS32 RPDO_COBId = 0x00000200 + nodeId;
			res = writeNetworkDictCallBack(	d, /*CO_Data* d*/
																			/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
																			nodeId, /*UNS8 nodeId*/
																			0x1400, /*UNS16 index*/
																			0x01, /*UNS8 subindex*/
																			4, /*UNS8 count*/
																			0, /*UNS8 dataType*/
																			&RPDO_COBId,/*void *data*/
																			CheckSDOAndContinue, /*SDOCallback_t Callback*/
																			0); /* use block mode */
		}			
		break;		
		
		case 7: 
		{	/*re-enable Slave's RPDO 1 */
			UNS32 Transmission_Type = 0xFE;
			res = writeNetworkDictCallBack(	d, /*CO_Data* d*/
																			/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
																			nodeId, /*UNS8 nodeId*/
																			0x1400, /*UNS16 index*/
																			0x02, /*UNS8 subindex*/
																			1, /*UNS8 count*/
																			0, /*UNS8 dataType*/
																			&Transmission_Type,/*void *data*/
																			CheckSDOAndContinue, /*SDOCallback_t Callback*/
																			0); /* use block mode */
		}			
		break;	
		
		case 8: 
		{	/*re-enable Slave's RPDO 1 */
			UNS32 Mapping_lenght = 0x1;
			res = writeNetworkDictCallBack(	d, /*CO_Data* d*/
																			/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
																			nodeId, /*UNS8 nodeId*/
																			0x1600, /*UNS16 index*/
																			0x0, /*UNS8 subindex*/
																			1, /*UNS8 count*/
																			0, /*UNS8 dataType*/
																			&Mapping_lenght,/*void *data*/
																			CheckSDOAndContinue, /*SDOCallback_t Callback*/
																			0); /* use block mode */
		}			
		break;
		
		case 9: 
		{	/*re-enable Slave's RPDO 1 */
			UNS32 Mapping_Paramete = 0x60FF0010;
			res = writeNetworkDictCallBack(	d, /*CO_Data* d*/
																			/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
																			nodeId, /*UNS8 nodeId*/
																			0x1600, /*UNS16 index*/
																			0x01, /*UNS8 subindex*/
																			4, /*UNS8 count*/
																			0, /*UNS8 dataType*/
																			&Mapping_Paramete,/*void *data*/
																			CheckSDOAndContinue, /*SDOCallback_t Callback*/
																			0); /* use block mode */
		}			
		break;
		
		case 10:
			init_step = 0;
			/* Put the master in operational mode */
			//setState(d, Operational);
			/* Ask slave node to go in operational mode */
			//masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
		break;
	}
			
}

/**/
static void CheckSDOAndOperation(CO_Data* d, UNS8 nodeId)
{
	UNS32 abortCode;

	if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) 
	{
		//eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
	}
	/* Finalise last SDO transfer with this node */
	closeSDOtransfer(&ObjDict_Data, nodeId, SDO_CLIENT);

	//Motec_Operation(nodeId);
}

void Motec_Disabled(UNS32 nodeId)
{
	UNS8 res;
	UNS32 Operation_Paramete = 0x6;
	UNS32 abortCode;
	res = writeNetworkDict(	&ObjDict_Data,
													nodeId,
													0x6040, 
													0x0, 
													2, 
													0,
													&Operation_Paramete, 
													0);
	while(getWriteResultNetworkDict (&ObjDict_Data, nodeId, &abortCode) == SDO_DOWNLOAD_IN_PROGRESS);
	motec_status[nodeId-1] = 0;
}

UNS8 Motec_Operation(UNS32 nodeId)
{
	UNS16 status = 0;
	UNS32 size;
	UNS32 abortCode;
	UNS8 res;
	size = sizeof (status);
	
	int step = 8;
	do {
		step--;
		res = ReadSDO(nodeId, 0x6041, 0, uint16, &status, &size);
		if(((UNS8)status&0x4F) == 0x0)	//Not ready to switch on
		{

		}
		else if(((UNS8)status&0x4F) == 0x40)	//Switch on disabled
		{
			motec_status[nodeId-1] = 0x50;
			UNS32 Operation_Paramete = 0x6;
			res = writeNetworkDict(	&ObjDict_Data,
												nodeId,
												0x6040, 
												0x0, 
												2, 
												0,
												&Operation_Paramete, 
												0);
			while(getWriteResultNetworkDict (&ObjDict_Data, nodeId, &abortCode) == SDO_DOWNLOAD_IN_PROGRESS);
			delay_ms(10);
		}
		else if(((UNS8)status&0x6F) == 0x21)	//Ready to switch on
		{
			motec_status[nodeId-1] = 0x21;
			UNS32 Operation_Paramete = 0x7;
			res = writeNetworkDict(	&ObjDict_Data, /*CO_Data* d*/
															nodeId, /*UNS8 nodeId*/
															0x6040, /*UNS16 index*/
															0x0, /*UNS8 subindex*/
															2, /*UNS8 count*/
															0, /*UNS8 dataType*/
															&Operation_Paramete,/*void *data*/
															0); /* use block mode */
			while(getWriteResultNetworkDict (&ObjDict_Data, nodeId, &abortCode) == SDO_DOWNLOAD_IN_PROGRESS);
			delay_ms(10);
		}
		else if(((UNS8)status&0x6F) == 0x23)	//Switch on
		{
			motec_status[nodeId-1] = 0x23;
			UNS32 Operation_Paramete = 0xF;
			res = writeNetworkDict(	&ObjDict_Data, /*CO_Data* d*/
															nodeId, /*UNS8 nodeId*/
															0x6040, /*UNS16 index*/
															0x0, /*UNS8 subindex*/
															2, /*UNS8 count*/
															0, /*UNS8 dataType*/
															&Operation_Paramete,/*void *data*/
															0); /* use block mode */
			while(getWriteResultNetworkDict (&ObjDict_Data, nodeId, &abortCode) == SDO_DOWNLOAD_IN_PROGRESS);
			delay_ms(10);
		}
		else if(((UNS8)status&0x6F) == 0x27)	//Operation enabled
		{
			motec_status[nodeId-1] = 0x27;
			return 1;
		}
		else if(((UNS8)status&0x6F) == 0x07)	//Quick stop active
		{
		
		}
		else if(((UNS8)status&0x4F) == 0x0F)	//Fault reaction active
		{
		
		}
		else if(((UNS8)status&0x4F) == 0x08)	//Fault
		{
		
		}
	} while(step);
	return 0;
}

void Master_heartbeatError(CO_Data* d, UNS8 heartbeatID)
{
	;;//eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
}

/********************************************************
 * ConfigureSlaveNode is responsible to
 *  - setup master RPDO 1 to receive TPDO 1 from id 2
 *  - setup master RPDO 2 to receive TPDO 2 from id 2
 ********************************************************/
void Master_initialisation(CO_Data* d)
{
	UNS32 PDO1_COBID = 0x0180 + 1;
	UNS32 size;

		/*****************************************
	 * Define RPDOs to match slave ID=0x40 TPDOs*
	 *****************************************/
	writeLocalDict(	&ObjDict_Data, /*CO_Data* d*/
									0x1400, /*UNS16 index*/
									0x01, /*UNS8 subind*/ 
									&PDO1_COBID, /*void * pSourceData,*/ 
									&size, /* UNS8 * pExpectedSize*/
									RW);  /* UNS8 checkAccess */
					
}

void Master_preOperational(CO_Data* d)
{
	;//eprintf("TestMaster_preOperational\n");
}

void Master_operational(CO_Data* d)
{
	;//eprintf("TestMaster_operational\n");
}

void Master_stopped(CO_Data* d)
{
	;//eprintf("TestMaster_stopped\n");
}

void Master_post_sync(CO_Data* d)
{
	;//eprintf("TestMaster_post_sync\n");
}

void Master_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg, const UNS8 errSpec[5])
{
	;//eprintf("Master received EMCY message. Node: %2.2x  ErrorCode: %4.4x  ErrorRegister: %2.2x\n", nodeID, errCode, errReg);
}

void Master_post_TPDO(CO_Data* d)
{

}

void Master_post_SlaveBootup(CO_Data* d, UNS8 nodeid)
{
	ConfigureSlaveNode(&ObjDict_Data, nodeid);
	//check_and_start_node(&ObjDict_Data, nodeid);
}

void Master_post_SlaveStateChange(CO_Data* d, UNS8 nodeId, e_nodeState newNodeState)
{
	if(newNodeState == Initialisation)
	{
		//ConfigureSlaveNode(&ObjDict_Data, nodeId);
		//printf("Node %u state is now  : Initialisation\n", nodeId);
	}
	else if(newNodeState == Disconnected)
		;//printf("Node %u state is now  : Disconnected\n", nodeId);
	else if(newNodeState == Connecting)
		;//printf("Node %u state is now  : Connecting\n", nodeId);
	else if(newNodeState == Preparing)
		;//printf("Node %u state is now  : Preparing\n", nodeId);
	else if(newNodeState == Stopped)
		;//printf("Node %u state is now  : Stopped\n", nodeId);
	else if(newNodeState == Operational)
	{
		//printf("Node %u state is now  : Operational\n", nodeId);
	}
	else if(newNodeState == Pre_operational)
	{
		;//printf("Node %u state is now  : Pre_operational\n", nodeId);
	}
	else if(newNodeState == Unknown_state)
		;//printf("Node %u state is now  : Unknown_state\n", nodeId);
	else
		;//printf("Error : node %u unexpected state\n", nodeId);
}

